'''
This demo show the communication interface of MR813 motion control board based on Lcm.
Dependency: 
- robot_control_cmd_lcmt.py
- robot_control_response_lcmt.py

雷达参数已经能够读取
替换basic_motion_test_comb中的main.py
运行代码之前需要执行:
cd /home/cyberdog_sim
source /opt/ros/galactic/setup.bash

修改代码内容：
删去全局变量 odo_rec  使用self.odo_msg代替
删去全局变量 motion_rec  使用self.rec_msg代替
err_handler函数里面的 motion_rec['switch_status'] 改为 self.rec_msg.switch_status

walk_forward函数中的 left_dist 名称改为 exp_dist
odo_changeback 更名为 odo_verticalturn
删去了无用的读锁 self.handle_lock

selaction 更名为 selfdef_action
side_equal 更名为 bothside_align


while ... return ... 改 if 的函数  ： odo_verticalturn   walk_forward   rightside_align

删去 turnleft_corner 函数（我当时写了一半没接着写了

speedump_and_circle  更名为   speedbump_and_circle （打错了

我写的大多数函数前面都加了些简单注释

路径/home/mi/Downloads/123/

'''
import lcm
import sys
import os
import time
from threading import Thread, Lock

from robot_control_cmd_lcmt import robot_control_cmd_lcmt
from robot_control_response_lcmt import robot_control_response_lcmt
from localization_lcmt import localization_lcmt

import toml
import copy
import math
from file_send_lcmt import file_send_lcmt

import rclpy
from rclpy.executors import MultiThreadedExecutor 
import numpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan,Image
from rclpy.qos import QoSProfile, qos_profile_sensor_data
from pprint import pprint
import cv2
from cv_bridge import CvBridge

# 自定义步态发送数据的结构
robot_cmd = {
    'mode': 0, 'gait_id': 0, 'contact': 0, 'life_count': 0,
    'vel_des': [0.0, 0.0, 0.0],
    'rpy_des': [0.0, 0.0, 0.0],
    'pos_des': [0.0, 0.0, 0.0],
    'acc_des': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    'ctrl_point': [0.0, 0.0, 0.0],
    'foot_pose': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    'step_height': [0.0, 0.0],
    'value': 0, 'duration': 0
}

PI = 3.1416
# R = 0.5
# T = 10
# 雷达数据
laser_rec = {
    'data': [0] * 500
}


# 激光雷达订阅
class LaserScanSubscriber(Node):

    def __init__(self):
        super().__init__('laser_scan_subscriber')
        # 使用SensorDataQoS作为QoS配置文件  
        qos_profile = qos_profile_sensor_data  # 这就是SensorDataQoS  
        self.subscription = self.create_subscription(
            LaserScan,
            '/mi_desktop_48_b0_2d_7a_fb_86/scan',  # 替换为你的激光雷达数据发布的话题名  
            self.listener_callback,
            qos_profile)

    def listener_callback(self, msg):
        global laser_rec
        laser_rec['data'] = msg.ranges.tolist()
        time.sleep(3)

class RGB_Subscriber(Node):
    def __init__(self):
        super().__init__('RGB_subscriber')
        qos_profile = qos_profile_sensor_data
        self.subscription = self.create_subscription(
            Image,
            '/image_rgb',
            self.listener_callback,
            qos_profile)
        self.bridge = CvBridge()
        
    #虚拟环境red_range = [20, 35], green_range = [50, 80]

    def find_green_location(self, image, red_range = [130, 140], green_range = [180, 185]): #调参部分1###################################
        #8.12下午13：30测试：红布:red_range = [180, 185], green_range = [120, 130]
        #8.12下午13：30测试：绿布:red_range = [130, 140], green_range = [180, 185]
        height, width = image.shape[:2]
        central_part = [2 * width // 8, 4 * width // 8, 0, 4 * height // 8] #调参部分2
        
        central_image = image[central_part[2]:central_part[3], central_part[0]:central_part[1]]
        # cv2.imshow("image",central_image)
        # cv2.waitKey(0)
        _,g,r = cv2.split(central_image)
        mean_green = numpy.mean(g)
        mean_red = numpy.mean(r)
        
        if green_range[0] <= mean_green <= green_range[1] and red_range[0] <= mean_red <= red_range[1]:  # 如果x坐标的平均值大于图像宽度的一半  
            return 1 
            
        else:  
            return -1
    
    def mid_before_bridge():
        return 

    def listener_callback(self, msg):
        # Convert the ROS Image message to an OpenCV image
        cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
        # cv2.imwrite('image_red.jpg',cv_image)
        self.RGB_res = self.find_green_location(cv2.imread("./image_green.jpg"))  
        # cv2.imwrite("image.jpg",cv_image)
        time.sleep(3)
        



class Robot_Ctrl(object):
    def __init__(self):
        # 反馈线程初始化
        self.rec_thread = Thread(target=self.rec_responce)
        self.send_thread = Thread(target=self.send_publish)
        self.odo_thread = Thread(target=self.rec_responce_o)
        self.lc_r = lcm.LCM("udpm://239.255.76.67:7670?ttl=255")
        self.lc_s = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")

        self.cmd_msg = robot_control_cmd_lcmt()
        self.rec_msg = robot_control_response_lcmt()
        self.odo_msg = localization_lcmt()
        self.send_lock = Lock()

        self.delay_cnt = 0
        self.mode_ok = 0
        self.gait_ok = 0
        self.runing = 1
        self.RGB_res = 0
        self.odo_bias = 0
        ####有bias之后，原先0度位置顺时针旋转bias(即变换到原bias位置)

        self.lc_o = lcm.LCM("udpm://239.255.76.67:7667?ttl=255")  ###里程计

        self.lcm_cmd = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
        self.lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
        self.usergait_msg = file_send_lcmt()

        rclpy.init(args=None)
        self.laser_scan_subscriber = LaserScanSubscriber()
        self.RGB_subscriber = RGB_Subscriber()
        
        self.executor = MultiThreadedExecutor()
        
        self.laser_thread = Thread(target=self.spin_func, args=(self.laser_scan_subscriber,))
        self.RGB_thread = Thread(target=self.spin_func, args=(self.RGB_subscriber,))
        
        self.camera_sh = Thread(target = os.system, args=('sh /home/mi/Downloads/motion/ros2.sh',))

    def spin_func(self, subscriber):
        self.executor.add_node(subscriber)
        self.executor.spin()
        time.sleep(0.02)

    def run_rgb(self):
        os.system('sh ./rgb_start.sh')
        self.RGB_thread.start()

    def run(self):
        self.lc_r.subscribe("robot_control_response", self.msg_handler)
        self.lc_o.subscribe("global_to_robot", self.msg_handler_o)  ###里程计订阅话题
        self.send_thread.start()
        self.rec_thread.start()
        self.odo_thread.start()  ###启动里程计

        self.laser_thread.start()
        self.camera_sh.start()
        
    def msg_handler(self, channel, data):
        self.rec_msg = robot_control_response_lcmt().decode(data)
        if (self.rec_msg.order_process_bar >= 95):
            self.mode_ok = self.rec_msg.mode
        else:
            self.mode_ok = 0

    def msg_handler_o(self, channel, data):  ###里程计解码函数，被按照频率调用
        self.odo_msg = localization_lcmt().decode(data)

    def rec_responce(self):
        while self.runing:
            self.lc_r.handle()
            time.sleep(0.002)

    def rec_responce_o(self):  ###里程计
        while self.runing:
            self.lc_o.handle()
            time.sleep(0.002)

    def selfdef_action(self, mode):  # 自定义步态选择参数
        try:
            self.send_lock.acquire()
            if mode == 0:  # 石子路+上下坡
                steps = toml.load("/home/mi/Downloads/motion/Gait_Params_walk.toml")
            elif mode == 1:  # 待定
                steps = toml.load(
                    "/home/mi/Downloads/motion/Gait_Params_highwalk.toml")
            full_steps = {'step': [robot_cmd]}
            k = 0
            for i in steps['step']:
                cmd = copy.deepcopy(robot_cmd)
                cmd['duration'] = i['duration']
                if i['type'] == 'usergait':
                    cmd['mode'] = 11  # LOCOMOTION
                    cmd['gait_id'] = 110  # USERGAIT
                    cmd['vel_des'] = i['body_vel_des']
                    cmd['rpy_des'] = i['body_pos_des'][0:3]
                    cmd['pos_des'] = i['body_pos_des'][3:6]
                    cmd['foot_pose'][0:2] = i['landing_pos_des'][0:2]
                    cmd['foot_pose'][2:4] = i['landing_pos_des'][3:5]
                    cmd['foot_pose'][4:6] = i['landing_pos_des'][6:8]
                    cmd['ctrl_point'][0:2] = i['landing_pos_des'][9:11]
                    cmd['step_height'][0] = math.ceil(i['step_height'][0] * 1e3) + math.ceil(
                        i['step_height'][1] * 1e3) * 1e3
                    cmd['step_height'][1] = math.ceil(i['step_height'][2] * 1e3) + math.ceil(
                        i['step_height'][3] * 1e3) * 1e3
                    cmd['acc_des'] = i['weight']
                    cmd['value'] = i['use_mpc_traj']
                    cmd['contact'] = math.floor(i['landing_gain'] * 1e1)
                    cmd['ctrl_point'][2] = i['mu']
                if k == 0:
                    full_steps['step'] = [cmd]
                else:
                    full_steps['step'].append(cmd)
                k = k + 1
            if mode == 0:  # 石子路+上下坡
                f = open("/home/mi/Downloads/motion/Gait_Params_walk_full.toml",
                         'w')
            elif mode == 1:  # 待定
                f = open("/home/mi/Downloads/motion/Gait_Params_highwalk_full.toml",
                         'w')
            f.write("# Gait Params\n")
            f.writelines(toml.dumps(full_steps))
            f.close()

            if mode == 0:  # 石子路+上下坡
                file_obj_gait_def = open(
                    "/home/mi/Downloads/motion/Gait_Def_walk.toml", 'r')
                file_obj_gait_params = open(
                    "/home/mi/Downloads/motion/Gait_Params_walk_full.toml", 'r')
            elif mode == 1:  # 待定
                file_obj_gait_def = open(
                    "/home/mi/Downloads/motion/Gait_Def_highwalk.toml", 'r')
                file_obj_gait_params = open(
                    "/home/mi/Downloads/motion/Gait_Params_highwalk_full.toml", 'r')
            self.usergait_msg.data = file_obj_gait_def.read()

            self.lcm_usergait.publish("user_gait_file", self.usergait_msg.encode())

            time.sleep(0.5)
            self.usergait_msg.data = file_obj_gait_params.read()
            self.lcm_usergait.publish("user_gait_file", self.usergait_msg.encode())
            time.sleep(0.1)
            file_obj_gait_def.close()
            file_obj_gait_params.close()
            if mode == 0:  # 石子路+上下坡
                user_gait_list = open(
                    "/home/mi/Downloads/motion/Usergait_List.toml", 'r')
            if mode == 1:  # 石子路+上下坡
                user_gait_list = open(
                    "/home/mi/Downloads/motion/Usergait_List1.toml", 'r')
            steps = toml.load(user_gait_list)
            for step in steps['step']:
                self.cmd_msg.mode = step['mode']
                self.cmd_msg.value = step['value']
                self.cmd_msg.contact = step['contact']
                self.cmd_msg.gait_id = step['gait_id']
                self.cmd_msg.duration = step['duration']
                self.cmd_msg.life_count += 1
                for i in range(3):
                    self.cmd_msg.vel_des[i] = step['vel_des'][i]
                    self.cmd_msg.rpy_des[i] = step['rpy_des'][i]
                    self.cmd_msg.pos_des[i] = step['pos_des'][i]
                    self.cmd_msg.acc_des[i] = step['acc_des'][i]
                    self.cmd_msg.acc_des[i + 3] = step['acc_des'][i + 3]
                    self.cmd_msg.foot_pose[i] = step['foot_pose'][i]
                    self.cmd_msg.ctrl_point[i] = step['ctrl_point'][i]
                for i in range(2):
                    self.cmd_msg.step_height[i] = step['step_height'][i]
                self.lcm_cmd.publish("robot_control_cmd", self.cmd_msg.encode())
                time.sleep(0.1)
            if mode == 0:  # 石子路+上下坡
                for i in range(
                        550):  # 10s Heat beat It is used to maintain the heartbeat when life count is not updated
                    self.lcm_cmd.publish("robot_control_cmd", self.cmd_msg.encode())
                    # self.err_handler(msg = robot_control_cmd_lcmt())
                    time.sleep(0.2)
            if mode == 1:  # 台阶
                for i in range(50):  # 10s Heat beat It is used to maintain the heartbeat when life count is not updated
                    self.lcm_cmd.publish("robot_control_cmd", self.cmd_msg.encode())
                # self.err_handler(msg = robot_control_cmd_lcmt())
                time.sleep(0.2)
            self.send_lock.release()
        except KeyboardInterrupt:
            self.cmd_msg.mode = 7  # PureDamper before KeyboardInterrupt:
            self.cmd_msg.gait_id = 0
            self.cmd_msg.duration = 0
            self.cmd_msg.life_count += 1
            self.lcm_cmd.publish("robot_control_cmd", self.cmd_msg.encode())
            self.send_lock.release()
            pass

    def Wait_finish(self, mode, gait_id, timeout=2000):  # timeout // 0.005 = 多少秒超时
        count = 0
        while self.runing and count < timeout:  # 10s
            if self.mode_ok == mode and self.gait_ok == gait_id:
                return True
            else:
                time.sleep(0.005)
                count += 1

    def send_publish(self):
        while self.runing:
            self.send_lock.acquire()
            if self.delay_cnt > 20:  # Heartbeat signal 10HZ, It is used to maintain the heartbeat when life count is not updated
                self.lc_s.publish("robot_control_cmd", self.cmd_msg.encode())
                self.delay_cnt = 0
            self.delay_cnt += 1
            self.send_lock.release()
            time.sleep(0.005)

    def Send_cmd(self, msg):
        self.send_lock.acquire()
        self.delay_cnt = 50
        self.cmd_msg = msg
        self.send_lock.release()

    def quit(self):
        self.runing = 0
        self.rec_thread.join()
        self.send_thread.join()
        self.odo_thread.join()
        self.laser_thread.join()
        self.camera_sh.join()

    def quit_rgb(self):
        os.system('sh ./rgb_end.sh')
        self.RGB_thread.join()

    def jump(self, msg, duration, type):  # 跳跃
        msg.mode = 16  # Locomotion
        msg.gait_id = type  # 0左跳 1前跳 3右跳 6原地跳
        msg.duration = duration  # Zero duration means continuous motion until a new command is used.
        # Continuous motion can interrupt non-zero duration interpolation motion
        msg.life_count += 1
        self.Send_cmd(msg)
        self.Wait_finish(16, 1)
        time.sleep(0.1)

    def Recovery_stand(self, msg):#恢复站立
        msg.mode = 12  # Recovery stand
        msg.gait_id = 0
        msg.life_count += 1  # Command will take effect when life_count update
        self.Send_cmd(msg)
        self.Wait_finish(12, 0)

    def Pure_Damp(self, msg, mode):
        msg.mode = 7  # PureDamp
        msg.gait_id = mode  # 0为自然倒下，1为受控倒下
        msg.life_count += 1  # Command will take effect when life_count update
        self.Send_cmd(msg)
        self.Wait_finish(12, 0)

    def circle(self, msg, R=0.6, T=12):  # 半径，时间
        msg.mode = 11  # Locomotion
        msg.gait_id = 10  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        msg.vel_des = [PI * R * 2 / T, 0, 2 * PI / T]  # forward  left/rightmove  rotate
        msg.duration = T * 550  # Zero duration means continuous motion until a new command is used.
        # Continuous motion can interrupt non-zero duration interpolation motion
        msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
        msg.life_count += 1
        msg.rpy_des = [0, 0.0, 0]
        self.Send_cmd(msg)
        self.Wait_finish(11, 10)

    def stone(self, msg):
        self.Recovery_stand(msg)
        self.selfdef_action(0)
        time.sleep(0.1)

    # def locomotion(s)
    def slope(self, msg):
        self.Recovery_stand(msg)
        self.selfdef_action(1)

    def err_handler(self, msg):
        if self.rec_msg.switch_status == 3:  # 如果状态为高阻尼保护模式
            msg.mode = 12  # Recovery stand
            msg.gait_id = 0
            msg.life_count += 1  # Command will take effect when life_count update
            self.Send_cmd(msg)
            self.Wait_finish(12, 0)

    # 雷达以及里程计
    # 探测直线的时候需要的二分查找
    def bio_select(self, k, b, start, end, dist_limit, data_with_order_x, data_with_order_y):
        left, right = start, end - 1

        # 二分查找
        while left < right:
            mid = (left + right) // 2
            x_mid, y_mid = data_with_order_x[mid], data_with_order_y[mid]

            # 计算点到直线的距离
            distance = abs(k * x_mid - y_mid + b) / (k ** 2 + 1) ** 0.5
            if distance <= dist_limit:
                # 点在直线附近，继续向右搜索
                left = mid + 1
            else:
                # 点不在直线附近，缩小搜索范围
                right = mid
        # 返回直线附近点的范围
        return left
    
    def detect_lines(self,max_limit = 12):  # data 实际雷达扫描的数组
    ####   此探测直线函数只在实体环境中使用！！！  此函数只在实体环境中使用！！！   此函数只在实体环境中使用！！！
        try:
            dist_limit = 0.16
            backward_limit = 0.04
            init_points = 40
            data_with_order_x = []
            data_with_order_y = []
            data = laser_rec['data']
            len_data = len(data)
            # print("data#####################################",data)
            # 过滤无效值
            for i in range(len_data):
                if data[i] < 0.08 or data[i] > max_limit:
                    continue
                # data_with_order.append([math.radians(i-90) - degree,data[i]])  #新坐标系里面的偏转角（弧度）+ 距离
                data_with_order_x.append(data[i] * math.cos(math.radians(i * 180 / len_data - 90)))
                data_with_order_y.append(data[i] * math.sin(math.radians(i * 180 / len_data - 90)))  # x y 轴
            len_filtered = len(data_with_order_x)

            # print(len(data_with_order_x))
            # for i in range(0, len(data_with_order_x)):
            #     print('(', end='')
            #     print(data_with_order_x[i], data_with_order_y[i], sep=' ,', end='')
            #     print('),', end='\n')
            # 储存直线方程
            lines = []
            # 拐动位置
            corners = [0]
            while corners[-1] < len_filtered - init_points - 1:
                start = corners[-1]
                k1_init, b_init = numpy.polyfit(data_with_order_x[start:start + init_points],
                                                data_with_order_y[start:start + init_points], 1)

                # print(k1_init, b_init)
                # 二分查找 end 是范围内最后一个点
                end = self.bio_select(k1_init, b_init, start, len_filtered, dist_limit, data_with_order_x,
                                    data_with_order_y)

                # 初次允许偏差稍大，可以防止有需要的点没能落在直线上，即升温
                k1_fixed, b1_fixed = numpy.polyfit(data_with_order_x[start:end], data_with_order_y[start:end], 1)
                dist_bias = (abs(k1_fixed * data_with_order_x[end] - data_with_order_y[end] + b1_fixed) /
                            (k1_fixed ** 2 + 1) ** 0.5)
                ######################################复活赛贴上来：
                if dist_bias < dist_limit:
                    end_new = self.bio_select(k1_fixed, b1_fixed, end, len_filtered, dist_limit, data_with_order_x,
                                            data_with_order_y)
                    k1_fixed, b1_fixed = numpy.polyfit(data_with_order_x[start:end_new],
                                                    data_with_order_y[start:end_new], 1)
                else:
                    end_new = end
                ######################################复活赛结束

                # ###进行退火，降低限制的范围，找到更加精确的点
                while dist_bias >= backward_limit:
                    end_new -= 2
                    dist_bias = (abs(k1_fixed * data_with_order_x[end_new] - data_with_order_y[end_new] + b1_fixed) /
                                (k1_fixed ** 2 + 1) ** 0.5)

                # 重新拟合直线
                k1_fixed, b1_fixed = numpy.polyfit(data_with_order_x[start:end_new], data_with_order_y[start:end_new], 1)
                corners.append(end_new)
                # print(end)
                lines.append([k1_fixed, b1_fixed])
                if len(lines) >= 3:
                    # print(corners)
                    print(lines)
                    break

            for i in lines:
                # print(f'{i[0]}x  - y + {i[1]} = 0')
                print(f'[{i[0]}, {i[1]}],')
            if len(lines)>=2:
                return lines,corners
            # else:
            #     time.sleep(0.04)
            #     return self.detect_lines()
        

        except:
            time.sleep(0.04)
            return self.detect_lines(max_limit = max_limit)
        
    # def detect_lines(self):  # data 180个的数组
    #     try:
    #         dist_limit = 0.12
    #         backward_limit = 0.04
    #         init_points = 5
    #         data_with_order_x = []
    #         data_with_order_y = []
    #         data = laser_rec['data']

    #         # 过滤无效值
    #         for i in range(180):
    #             if data[i] < 0.12 or data[i] > 12:
    #                 continue
    #             # data_with_order.append([math.radians(i-90) - degree,data[i]])  #新坐标系里面的偏转角（弧度）+ 距离
    #             data_with_order_x.append(data[i] * math.cos(math.radians(i - 90)))
    #             data_with_order_y.append(data[i] * math.sin(math.radians(i - 90)))  # x y 轴
    #         len_filtered = len(data_with_order_x)

    #         # print(len(data_with_order_x))
    #         # for i in range(0, len(data_with_order_x)):
    #         #     print('(', end='')
    #         #     print(data_with_order_x[i], data_with_order_y[i], sep=' ,', end='')
    #         #     print('),', end='\n')
    #         # 储存直线方程
    #         lines = []
    #         # 拐动位置
    #         corners = [0]
    #         while corners[-1] < len_filtered - init_points - 1:
    #             start = corners[-1]
    #             k1_init, b_init = numpy.polyfit(data_with_order_x[start:start + init_points],
    #                                             data_with_order_y[start:start + init_points], 1)

    #             # print(k1_init, b_init)
    #             # 二分查找 end 是范围内最后一个点
    #             end = self.bio_select(k1_init, b_init, start, len_filtered, dist_limit, data_with_order_x,
    #                                   data_with_order_y)

    #             # 初次允许偏差稍大，可以防止有需要的点没能落在直线上，即升温
    #             k1_fixed, b1_fixed = numpy.polyfit(data_with_order_x[start:end], data_with_order_y[start:end], 1)
    #             dist_bias = (abs(k1_fixed * data_with_order_x[end] - data_with_order_y[end] + b1_fixed) /
    #                          (k1_fixed ** 2 + 1) ** 0.5)

    #             end_new = end
    #             # ###进行退火，降低限制的范围，找到更加精确的点
    #             while dist_bias >= backward_limit:
    #                 end_new -= 1
    #                 dist_bias = (abs(k1_fixed * data_with_order_x[end_new] - data_with_order_y[end_new] + b1_fixed) /
    #                              (k1_fixed ** 2 + 1) ** 0.5)

    #             # 重新拟合直线
    #             k1_fixed, b1_fixed = numpy.polyfit(data_with_order_x[start:end_new], data_with_order_y[start:end_new], 1)
    #             corners.append(end_new)
    #             # print(end)
    #             lines.append([k1_fixed, b1_fixed])
    #             if len(lines) >= 3:
    #                 print(corners)
    #                 break

    #         # for i in lines:
    #         #     # print(f'{i[0]}x  - y + {i[1]} = 0')
    #         #     print(f'[{i[0]}, {i[1]}],')
    #         return lines
    #     ###try后面是原来的代码
    #     except:
    #         time.sleep(0.04)
    #         return self.detect_lines()


########该函数用右侧直线来作为基准，重新设置前面的正方向为里程计的 exp_angel 角度，默认设置为0度
########在使用此函数前，调用odo_verticalturn()先大致方向对正，再使用此函数微调
########在使用此函数后，调用odo_verticalturn(target = exp_angel)可以使机械狗正对前方
    def odo_parallel_line(self, exp_angel=0,manu_bias = 0):
        lines = self.detect_lines()[0]
        k = lines[0][0]
        k_angel = math.atan(k)  ###反正切算角度
        now_angel = self.odo_msg.rpy[2]  ###现在的里程计角度
        # now_bias = self.odo_bias   ###现在的偏置角度
        self.odo_bias = now_angel + k_angel - exp_angel + manu_bias
        # now_angel + k_angel 是以里程计0位 为基准的直线方向角度
        # - exp_angel之后得到的就是 里程计0位 和 期望0位 夹角

    # ####对于直角而言，修改后的里程计转动函数更加精确，一次性可以转到对应位置
    def circle_dist(self,target,location):
        ###3.1416   -3.1416   是同一个值
        value1 = abs(target - location)
        value2 = 6.2832 - value1
        direction1 = 1 if target > location else 0  # ##按value1转动，1为逆时针  0 为顺时针
        if value1 < value2:
            return direction1,value1

        else:
            return 1-direction1,value2

    def odo_verticalturn(self, target, msg, limit=0.04, timesleep_s=5):  ##允许误差0.04弧度，大概是2.29度
        target = target + self.odo_bias
        if target < -3.14159:
            target += 3.14159
        if target > 3.14159:
            target -= 3.14159
        # ###############坐标系转换回原来的值
        
        const_int = 2470  # 转 1.57 弧度 大概要 3875 duration  每弧度大概这个值  持续时间大概6.5秒
        loc = self.odo_msg.rpy[2]
        direction, dist = self.circle_dist(target=target, location=loc)
        print('odo_verticalturn', self.odo_msg.rpy[2], target)
        if abs(dist) > limit:
            msg.mode = 11  # Locomotion
            msg.gait_id = 26  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
            msg.vel_des = [0, 0, 0.5 if direction > 0 else -0.5]  # 转向
            msg.duration = int(const_int * abs(dist))
            # Continuous motion can interrupt non-zero duration interpolation motion
            msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
            msg.life_count += 1
            self.Send_cmd(msg)
            time.sleep(7 * abs(dist) / 1.57)
            # print('1 times finish angle',self.odo_msg.rpy[2])
            # return
            dist = target - self.odo_msg.rpy[2]
            print('odo_verticalturn', self.odo_msg.rpy[2], target)

    #####左右两侧的直线都可以扫描到时，可以使用这个函数来在路中对齐
    #####offset以左为正向，偏离中线的距离
    def bothside_align(self, msg, limit=0.08, off_set=0.0, timeout=600):  ## 允许最大误差0.08米，即离最中线左右偏向4厘米
        ###先里程计回正再算距离
        const_int = 6666  # 对 差的距离 进行倍数放大 控制左右移动时间
        lines = self.detect_lines()[0]
        while len(lines) < 3:
            time.sleep(0.04)
            lines = self.detect_lines()[0]
        dist1 = round(abs(lines[0][1] / ((lines[0][0] * lines[0][0] + 1)) ** 0.5), 3)
        dist2 = round(abs(lines[2][1] / ((lines[2][0] * lines[2][0] + 1)) ** 0.5), 3)
        # print(f'before move to equal: r :{dist1}   l: {dist2}')
        dist_diff = dist1 - dist2 - 2 * off_set
        while abs(dist_diff) > limit:
            msg.mode = 11  # Locomotion
            msg.gait_id = 27
            msg.vel_des = [0, -0.15, 0] if dist_diff > 0 else [0, 0.15, 0]
            msg.duration = int(const_int * abs(dist_diff) / 2)
            msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
            msg.life_count += 1
            msg.rpy_des = [0, 0.3, 0]
            self.Send_cmd(msg)
            time.sleep(msg.duration / 1000 + 0.5)
            time.sleep(3)
            lines = self.detect_lines()[0]
            while len(lines) < 3:
                time.sleep(0.04)
                lines = self.detect_lines()[0]
            dist1 = round(abs(lines[0][1] / ((lines[0][0] * lines[0][0] + 1)) ** 0.5), 3)
            dist2 = round(abs(lines[2][1] / ((lines[2][0] * lines[2][0] + 1)) ** 0.5), 3)
            dist_diff = dist1 - dist2

    ####前向运动函数需要在里程计回正之后才能使用
    def walk_forward(self, msg, exp_dist, limit=0.04, timeout=1200, k_bound=5):  ##在对正之后才能调用，exp_dist指定前面预留多长的距离
        const_int = 1667  # 对 差的距离 进行倍数放大 控制前进时间
        lines = self.detect_lines()[0]
        dist = round(abs(lines[1][1] / ((lines[1][0] * lines[1][0] + 1)) ** 0.5), 3)
        print(dist)
        # for i in lines:
        #     print(i)
        while (len(lines) <= 1 or (lines[1][0] < k_bound and lines[1][0] > -k_bound)):
            time.sleep(0.04)
            lines = self.detect_lines()[0]
            dist = round(abs(lines[1][1] / ((lines[1][0] * lines[1][0] + 1)) ** 0.5), 3)

        dist_need_walk = dist - exp_dist
        if abs(dist_need_walk) > limit:
            msg.mode = 11  # Locomotion
            msg.gait_id = 27
            msg.vel_des = [0.6 if dist_need_walk > 0 else -0.6, 0, 0]
            msg.duration = int(const_int * abs(dist_need_walk))
            msg.step_height = [0.06, 0.06]  # ground clearness of swing leg

            msg.life_count += 1
            self.Send_cmd(msg)
            self.Wait_finish(11, 27, timeout=timeout)

            lines = self.detect_lines()[0]
            dist = round(abs(lines[1][1] / ((lines[1][0] * lines[1][0] + 1)) ** 0.5), 3)
            # while (len(lines) <= 1 or (lines[1][0] <k_bound and lines[1][0]>-k_bound)):
            #     time.sleep(0.04)
            #     lines = self.detect_lines()
            #     dist = round(abs(lines[1][1] / ((lines[1][0] * lines[1][0] + 1)) ** 0.5),3)
            # dist_need_walk = dist - exp_dist
            print('walk_forward dis_after', dist)


    ####对部分地形，只使用扫描到的右侧直线即对正赛道，expect_dist为指定离右边直线的距离
    def rightside_align(self, msg, expect_dist=0.65, limit=0.04, timeout=800):
        const_int = 5000
        lines = self.detect_lines()[0]
        dist = round(abs(lines[0][1] / ((lines[0][0] * lines[0][0] + 1)) ** 0.5), 3)
        dist_need_walk = dist - expect_dist
        if abs(dist_need_walk) > limit:
            msg.mode = 11  # Locomotion
            msg.gait_id = 27
            msg.vel_des = [0, -0.2 if dist_need_walk > 0 else 0.2, 0]
            msg.duration = int(const_int * abs(dist_need_walk))
            msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
            msg.life_count += 1
            self.Send_cmd(msg)
            self.Wait_finish(11, 27, timeout=timeout)

            lines = self.detect_lines()[0]
            dist = round(abs(lines[0][1] / ((lines[0][0] * lines[0][0] + 1)) ** 0.5), 3)

            # dist_need_walk = dist - expect_dist

            # print('right align dis_after', dist)

    ###此函数用于小角度的纠正，精确程度更加高
    ###timesleep_s单位是秒，指定一次回正之后需要间隔多久，建议在verticval_turn之后使用，更加精确
    def odo_tinyturn(self, target, msg, limit=0.04, timesleep_s=5):
        const_int = 1200
        dist = target - self.odo_msg.rpy[2]
        print('odo_tinyturn', self.odo_msg.rpy[2], target)
        while abs(dist) > limit:
            msg.mode = 11  # Locomotion
            msg.gait_id = 26  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
            msg.vel_des = [0, 0, 0.5 if dist > 0 else -0.5]  # 转向
            msg.duration = int(const_int * abs(dist))
            # Continuous motion can interrupt non-zero duration interpolation motion
            msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
            msg.life_count += 1
            self.Send_cmd(msg)
            time.sleep(timesleep_s)
            # print('1 times finish angle',self.odo_msg.rpy[2])
            # return
            dist = target - self.odo_msg.rpy[2]
            print('odo_tinyturn', self.odo_msg.rpy[2], target)

    ###在第二个拐角对齐之后，可以调用这个函数。直接通过减速带和圆柱
    # def speedbump_and_circle(self, msg):
    #     const_int1 = 5000  #######在走向圆柱的情况下，使用的duration常数
    #     const_int2 = 5000  #######在侧向靠近圆柱，进入赛道位置的情况下，使用的duration常数
    #     y_sharp_dist = -0.08  #######给最后的对齐使用的参数   绝对值是对正之后拐点和狗y轴的差距   正常要负数
    #     x_sharp_dist = 0.30  #######给最后的对齐使用的参数   是对正之后拐点和狗x轴的差距 

    #     #####开环走过大部分减速带
    #     msg.mode = 11  # Locomotion
    #     msg.gait_id = 26  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
    #     msg.vel_des = [0.2, 0, 0]  # 直走
    #     msg.rpy_des = [0, 0, 0]
    #     # Zero duration means continuous motion until a new command is used.
    #     msg.duration = 14000
    #     # Continuous motion can interrupt non-zero duration interpolation motion
    #     msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
    #     msg.life_count += 1
    #     self.Send_cmd(msg)

    #     time.sleep(24)

    #     ###找到方向处理剩余的不确定距离，主要是向半径方向靠近圆柱
    #     time.sleep(0.04)
    #     laser_keep = laser_rec['data']
    #     idx_min = laser_keep[70:110].index(min(laser_keep[70:110])) + 70  # 最小值点
    #     dist_min = laser_keep[idx_min]
    #     angel_diff = idx_min / 90 * 1.57

    #     # print(idx_min, dist_min, angel_diff)

    #     if dist_min > 0.15:
    #         msg.mode = 11  # Locomotion
    #         msg.gait_id = 26  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
    #         msg.vel_des = [0.1 * math.sin(angel_diff), 0.1 * math.cos(angel_diff), 0]  # 直走
    #         msg.rpy_des = [0, 0, 0]
    #         # Zero duration means continuous motion until a new command is used.
    #         msg.duration = int((dist_min - 0.15) * const_int1)
    #         # Continuous motion can interrupt non-zero duration interpolation motion
    #         msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
    #         msg.life_count += 1
    #         self.Send_cmd(msg)
    #         # print('tiny walk')

    #     ####转向正右边，回正
    #     time.sleep(0.5)
    #     self.odo_verticalturn(1.5708, msg=msg)
    #     self.odo_tinyturn(target=1.5708, msg=msg)

    #     time.sleep(3)
    #     ###########################找右边直线和圆环赛道的交界点，以这个点为基准去进行对齐
    #     ###########################多次扫描去除错误点
    #     laser_valid = [0] * 180
    #     for i in range(10):
    #         time.sleep(0.04)
    #         laser = laser_rec['data']
    #         for j in range(0, 180):
    #             if laser_valid[j] == 0 and laser[j] > 0.15:
    #                 laser_valid[j] = laser[j]
    #     ##########################根据X轴的值去找到拐角点，序号idx
    #     data_with_order_x = []
    #     data_with_order_y = []

    #     for i in range(180):
    #         if laser_valid[i] < 0.12 or laser_valid[i] > 12:
    #             continue
    #         # data_with_order.append([math.radians(i-90) - degree,data[i]])  #新坐标系里面的偏转角（弧度）+ 距离
    #         data_with_order_x.append(laser_valid[i] * math.cos(math.radians(i - 90)))
    #         data_with_order_y.append(laser_valid[i] * math.sin(math.radians(i - 90)))  # x y 轴
    #     len_filtered = len(data_with_order_x)

    #     # print('len_filtered',len_filtered)
    #     # print(data_with_order_x)

    #     idx = -1
    #     i = 60
    #     while i < len_filtered - 10:
    #         cnt = 0
    #         for k in range(i + 1, i + 9):
    #             if data_with_order_x[k] > data_with_order_x[k - 1]:
    #                 cnt += 1
    #             else:
    #                 break
    #         if cnt > 6:
    #             idx = i
    #             break
    #         i += 1

    #     ###################读出拐角点的值
    #     point_sharp_x = data_with_order_x[idx]
    #     point_sharp_y = data_with_order_y[idx]

    #     # print('idx   pt_sharp_x', idx, point_sharp_x)
    #     # print('idx   pt_sharp_y', idx, point_sharp_y)

    #     time.sleep(1)

    #     #####根据拐角点矫正方位，先在y轴上矫正
    #     msg.mode = 11  # Locomotion
    #     msg.gait_id = 26  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
    #     msg.vel_des = [0, 0.2 if (point_sharp_y - y_sharp_dist) > 0 else -0.2, 0]
    #     msg.rpy_des = [0, 0, 0]
    #     # Zero duration means continuous motion until a new command is used.
    #     msg.duration = int(const_int2 * abs(y_sharp_dist - point_sharp_y))
    #     # Continuous motion can interrupt non-zero duration interpolation motion
    #     msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
    #     msg.life_count += 1
    #     self.Send_cmd(msg)
    #     time.sleep(5)

    #     ####再矫正x轴
    #     msg.mode = 11  # Locomotion
    #     msg.gait_id = 26  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
    #     msg.vel_des = [-0.2 if (point_sharp_x - x_sharp_dist) < 0 else 0.2, 0, 0]
    #     msg.rpy_des = [0, 0, 0]
    #     # Zero duration means continuous motion until a new command is used.
    #     msg.duration = int(const_int2 * abs(point_sharp_x - x_sharp_dist))
    #     # Continuous motion can interrupt non-zero duration interpolation motion
    #     msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
    #     msg.life_count += 1
    #     self.Send_cmd(msg)
    #     time.sleep(5)

    #     #####绕圈出去
    #     msg.mode = 11  # Locomotion
    #     msg.gait_id = 26  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
    #     msg.vel_des = [0.165, 0, 0.3]  # 转向
    #     msg.duration = 12000
    #     # Continuous motion can interrupt non-zero duration interpolation motion
    #     msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
    #     msg.life_count += 1
    #     self.Send_cmd(msg)

    #     time.sleep(20)

    def speedbump(self, msg):
        expect_wall_angel = -1.57  ####面相墙的方向的角度，即绕环开始前方向

        const_int1 = 5000  #######在走向圆柱的情况下，使用的duration常数
        const_int2 = 5000  #######在侧向靠近圆柱，进入赛道位置的情况下，使用的duration常数


        #####开环走过大部分减速带
        msg.mode = 11  # Locomotion
        msg.gait_id = 26  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        msg.vel_des = [0.2, 0, 0]  # 直走
        msg.rpy_des = [0, 0, 0]
        # Zero duration means continuous motion until a new command is used.
        msg.duration = 12000
        # Continuous motion can interrupt non-zero duration interpolation motion
        msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
        msg.life_count += 1
        self.Send_cmd(msg)

        time.sleep(18)

        ###找到方向处理剩余的不确定距离，主要是向半径方向靠近圆柱
        time.sleep(0.04)
        laser_keep = laser_rec['data']
        len_data = len(laser_keep)
        idx_min = laser_keep[210:330].index(min(laser_keep[210:330])) + 210  # 最小值点
        dist_min = laser_keep[idx_min]
        angel_diff = idx_min / 270 * 1.57  ###算出最短距离上的弧度制

        # print(idx_min, dist_min, angel_diff)

        if dist_min > 0.15:
            msg.mode = 11  # Locomotion
            msg.gait_id = 26  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
            msg.vel_des = [0.1 * math.sin(angel_diff), 0.1 * math.cos(angel_diff), 0]  # 直走
            msg.rpy_des = [0, 0, 0]
            # Zero duration means continuous motion until a new command is used.
            msg.duration = int((dist_min - 0.15) * const_int1)
            # Continuous motion can interrupt non-zero duration interpolation motion
            msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
            msg.life_count += 1
            self.Send_cmd(msg)
            # print('tiny walk')

        ####转向正右边，回正
        time.sleep(0.5)
        

    def pass_circle(self,msg):
        expect_wall_angel = 1.57  ####面相墙的方向的角度，即绕环开始前方向
        
        y_sharp_dist = 0.02  #######给最后的对齐使用的参数   绝对值是对正之后拐点和狗y轴的差距   正常要负数
        x_sharp_dist = 0.45  #######给最后的对齐使用的参数   是对正之后拐点和狗x轴的差距
        dist_limit = 0.20  #######计算点到直线的阈值，用来找到拐角点
        acc_points = 6    #######累计这么多点的时候判断到了弯道

        self.odo_verticalturn(expect_wall_angel, msg=msg)
        # self.odo_tinyturn(target=1.5708, msg=msg)

        time.sleep(3)
        ###########################找右边直线和圆环赛道的交界点，以这个点为基准去进行对齐
        ###########################多次扫描去除错误点
        # laser_valid = [0] * 450
        # for i in range(10):
        #     time.sleep(0.04)
        #     laser = laser_rec['data']
        #     for j in range(0, 450):
        #         if laser_valid[j] == 0 and laser[j] > 0.10:
        #             laser_valid[j] = laser[j]
        laser_valid = laser_rec['data']
        ##########################根据X轴的值去找到拐角点，序号idx
        data_with_order_x = []
        data_with_order_y = []

        for i in range(450):
            if laser_valid[i] < 0.06 or laser_valid[i] > 12:
                continue
            # data_with_order.append([math.radians(i-90) - degree,data[i]])  #新坐标系里面的偏转角（弧度）+ 距离
            data_with_order_x.append(laser_valid[i] * math.cos(math.radians(i * 0.36 - 90)))
            data_with_order_y.append(laser_valid[i] * math.sin(math.radians(i * 0.36 - 90)))  # x y 轴
        len_filtered = len(data_with_order_x)

        # -----------------------------------------------------新的获取idx的直线扫描方法
        
        lines_corners = self.detect_lines(max_limit = 6)
        while abs(lines_corners[0][0][0]) < 7:  ###直线  第一条  k
            time.sleep(0.04)
            lines_corners = self.detect_lines(max_limit = 6)

        lines = lines_corners[0]
        corners = lines_corners[1]

        dists = [0] * len_filtered

        # cnt = 0
        # idx = 250
        # for _ in range(250, min(len_filtered, 450)):
        #     #############abs(kx + b - y) / k ** 2 + 1
        #     dist = abs(lines[0][0] * data_with_order_x[_] + lines[0][1] - data_with_order_y[_]) / ((lines[0][0] ** 2 + 1) ** 0.5)
        #     if dist > dist_limit:  ####距离太大的时候计数
        #         dists[_] = dist
        #         cnt += 1
        #     else:
        #         cnt = 0

        #     if cnt >= acc_points:
        #         idx = _ - int(1.5 * acc_points)
        #         break

        idx = corners[1]

        point_sharp_x = data_with_order_x[idx]
        point_sharp_y = data_with_order_y[idx]
        print('idx   pt_sharp_x', idx, point_sharp_x)
        print('idx   pt_sharp_y', idx, point_sharp_y)

        for i in range(0, len(data_with_order_x)):
            print('(', end='')
            print(data_with_order_x[i], data_with_order_y[i], sep=' ,', end='')
            print('),', end='\n')
            
        print('lines',lines[0])
        print('len_filtered',len_filtered)
        print('绕障前侧墙距离',dists[250:])


        # -----------------------------------------------------新的获取idx的直线扫描方法

        time.sleep(1)

        #####根据拐角点矫正方位，先在y轴上矫正
        msg.mode = 11  # Locomotion
        msg.gait_id = 26  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        msg.vel_des = [0, 0.2 if (point_sharp_y - y_sharp_dist) > 0 else -0.2, 0]
        msg.rpy_des = [0, 0, 0]
        # Zero duration means continuous motion until a new command is used.
        msg.duration = int(const_int2 * abs(y_sharp_dist - point_sharp_y))
        # Continuous motion can interrupt non-zero duration interpolation motion
        msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
        msg.life_count += 1
        self.Send_cmd(msg)
        time.sleep(5)

        ####再矫正x轴
        msg.mode = 11  # Locomotion
        msg.gait_id = 26  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        msg.vel_des = [-0.2 if (point_sharp_x - x_sharp_dist) < 0 else 0.2, 0, 0]
        msg.rpy_des = [0, 0, 0]
        # Zero duration means continuous motion until a new command is used.
        msg.duration = int(const_int2 * abs(point_sharp_x - x_sharp_dist))
        # Continuous motion can interrupt non-zero duration interpolation motion
        msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
        msg.life_count += 1
        self.Send_cmd(msg)
        time.sleep(5)
        # -----------------------------------------------------
        return
        # -----------------------------------------------------
        #####绕圈出去
        msg.mode = 11  # Locomotion
        msg.gait_id = 26  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        msg.vel_des = [0.165, 0, 0.3]  # 转向
        msg.duration = 12000
        # Continuous motion can interrupt non-zero duration interpolation motion
        msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
        msg.life_count += 1
        self.Send_cmd(msg)

        time.sleep(20)
        self.odo_verticalturn(expect_wall_angel + 1.5708, msg=msg)

# Main function



def pass_cloth(Ctrl ,msg ,pos = -1):
        # Ctrl.Recovery_stand(msg)
        msg.mode = 11  # Locomotion
        msg.gait_id = 27
        msg.vel_des = [0.3, 0, 0]
        msg.duration = 1000 #3000
        msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(10)
        Ctrl.rightside_align(msg=msg,expect_dist=0.9)
        Ctrl.run_rgb()
        if pos < 0:
            print(Ctrl.RGB_res)
            if Ctrl.RGB_res > 0:
                Ctrl.rightside_align(msg=msg,expect_dist=0.9)
            else:  
                Ctrl.rightside_align(msg=msg,expect_dist=0.4)
                pos = 1
        else:
            if Ctrl.RGB_res > 0:
                Ctrl.rightside_align(msg=msg,expect_dist=0.4)
            else:  
                Ctrl.rightside_align(msg=msg,expect_dist=0.9)
                pos = -1
        msg.mode = 11  # Locomotion
        msg.gait_id = 27
        msg.vel_des = [0.3, 0, 0]
        msg.duration = 3333
        msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(10)
        if pos < 0:
            print(Ctrl.RGB_res)
            if Ctrl.RGB_res > 0:
                Ctrl.rightside_align(msg=msg,expect_dist=0.9)
            else:  
                Ctrl.rightside_align(msg=msg,expect_dist=0.4)
                pos = 1
        else:
            if Ctrl.RGB_res > 0:
                Ctrl.rightside_align(msg=msg,expect_dist=0.4)
            else:  
                Ctrl.rightside_align(msg=msg,expect_dist=0.9)
                pos = -1
        msg.mode = 11  # Locomotion
        msg.gait_id = 27
        msg.vel_des = [0.3, 0, 0]
        msg.duration = 3333
        msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(10)
        if pos < 0:
            print(Ctrl.RGB_res)
            if Ctrl.RGB_res > 0:
                Ctrl.rightside_align(msg=msg,expect_dist=0.9)
            else:  
                Ctrl.rightside_align(msg=msg,expect_dist=0.4)
                pos = 1
        else:
            if Ctrl.RGB_res > 0:
                Ctrl.rightside_align(msg=msg,expect_dist=0.4)
            else:  
                Ctrl.rightside_align(msg=msg,expect_dist=0.9)
                pos = -1
        msg.mode = 11  # Locomotion
        msg.gait_id = 27
        msg.vel_des = [0.3, 0, 0]
        msg.duration = 3333
        msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(10)
        if pos < 0:
            print(Ctrl.RGB_res)
            if Ctrl.RGB_res > 0:
                Ctrl.rightside_align(msg=msg,expect_dist=0.9)
            else:  
                Ctrl.rightside_align(msg=msg,expect_dist=0.4)
                pos = 1
        else:
            if Ctrl.RGB_res > 0:
                Ctrl.rightside_align(msg=msg,expect_dist=0.4)
            else:  
                Ctrl.rightside_align(msg=msg,expect_dist=0.9)
                pos = -1
        msg.mode = 11  # Locomotion
        msg.gait_id = 27
        msg.vel_des = [0.3, 0, 0]
        msg.duration = 3333
        msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        time.sleep(10)
        if pos < 0:
            print(Ctrl.RGB_res)
            if Ctrl.RGB_res > 0:
                Ctrl.rightside_align(msg=msg,expect_dist=0.9)
            else:  
                Ctrl.rightside_align(msg=msg,expect_dist=0.4)
                pos = 1
        else:
            if Ctrl.RGB_res > 0:
                Ctrl.rightside_align(msg=msg,expect_dist=0.4)
            else:  
                Ctrl.rightside_align(msg=msg,expect_dist=0.9)
                pos = -1
        msg.mode = 11  # Locomotion
        msg.gait_id = 27
        msg.vel_des = [0.3, 0, 0]
        msg.duration = 3333
        msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
        msg.life_count += 1
        Ctrl.Send_cmd(msg)
        Ctrl.quit_rgb()
        time.sleep(10)

def main():
    Ctrl = Robot_Ctrl()
    Ctrl.run()
    msg = robot_control_cmd_lcmt()
    try:
#############################################################################################################################
        Ctrl.Recovery_stand(msg)
        # msg.mode = 11  # Locomotion
        # msg.gait_id = 26  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        # msg.vel_des = [0.2, 0, 0]
        # msg.rpy_des = [0, 0, 0]
        # msg.pos_des = [0, 0, 0.1]
        # # Zero duration means continuous motion until a new command is used.
        # msg.duration = 10000
        # # Continuous motion can interrupt non-zero duration interpolation motion
        # msg.step_height = [0.15, 0.15]  # ground clearness of swing leg
        # msg.life_count += 1
        # Ctrl.Send_cmd(msg)
        # time.sleep(5)
        # time.sleep(3)
        # pass_cloth(Ctrl, msg)

#############################################################################################################################
        # Ctrl.RGB_res = Ctrl.RGB_subscriber.find_green_location(image = cv2.imread("./image_red.jpg"))
        # print(Ctrl.RGB_res)
        # Ctrl.selfdef_action(1)
        # Ctrl.circle(msg)

        # 上石子路
        # Ctrl.odo_verticalturn(0, msg=msg)
        # time.sleep(1)
        # Ctrl.walk_forward(msg=msg, exp_dist=0.60)
        # time.sleep(0.2)
        # Ctrl.odo_verticalturn(target=1.5707, msg=msg)
        # msg.mode = 11  # Locomotion
        # msg.gait_id = 10  # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26
        # msg.vel_des = [0.5, 0, 0]  # forward  left/rightmove  rotate
        # msg.duration = 3600  # Zero duration means continuous motion until a new command is used.
        # # Continuous motion can interrupt non-zero duration interpolation motion
        # msg.step_height = [0.06, 0.06]  # ground clearness of swing leg
        # msg.life_count += 1
        # msg.rpy_des = [0, 0.0, 0]
        # Ctrl.Send_cmd(msg)
        # Ctrl.Wait_finish(11, 10)
        # Ctrl.odo_verticalturn(target=1.5708, msg=msg)
        # time.sleep(0.1)
        # Ctrl.stone(msg)
        # Ctrl.Recovery_stand(msg)
        # time.sleep(3)
        # Ctrl.odo_verticalturn(target=1.5708, msg=msg)
        # time.sleep(3)
        # Ctrl.walk_forward(msg, exp_dist=0.4)
        # time.sleep(3)
        # Ctrl.rightside_align(msg=msg, expect_dist=1.15)
        # time.sleep(4)
        # Ctrl.odo_verticalturn(target=3.1415, msg=msg)
        # time.sleep(1)
        # Ctrl.speedbump_and_circle(msg)
        # Ctrl.jump(msg, 1200, 1)
        # Ctrl.odo_verticalturn(target=3.1415, msg=msg)
        # Ctrl.bothside_align(msg)
        # while True:
        # Ctrl.Recovery_stand(msg)
        # Ctrl.jump(msg,1200,1)
        # time.sleep(0.1)


    except KeyboardInterrupt:
        pass
    Ctrl.quit()
    sys.exit()

if __name__ == '__main__':
    main()